#include "modules/CtrlModule/PassivePlanner/TopCapObsPlanner.h"
#include "lib/WrapFuncs.h"
#include "data_struct/geometry/Point.h"
#include "data_struct/robot/Pose.h"
#include "lib/Time.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
    
    #define TURN_REEOR 0.052359876
    #define POINT_NUM 5
    TopCapObsPlanner top_cap_obs_planner;
    void TopCapObsPlanner::run(double &linear_vel, double &angular_vel)
    {
        PointCloud<double> in_contour_points;
        int point_count = CheakTopCapObs(in_contour_points);
        
        if (point_count > POINT_NUM || !avoid_finished) //在5个点以上就继续后退，小于10个点时已不影响机器人转向
        {
            Avoid(linear_vel, angular_vel);
            last_angle = DataSet::robot_pose.angle();
        }
        else
        { 
            if(obs_type== DYNAMIC_OBS)
            {
                finished_turn=true;
            }
            if (!init_angle)
            {
                std::cout<< "top_cap_obs_planner init_angle! "<< " meanx= " << meanx << " meany= " << meany <<std::endl;
                init_angle=true;
                PoseF robot_pose_(0.0f, 0.0f, .0f),end_point(meanx,meany, .0f);
                double local_angle= std::atan2(end_point.y()-robot_pose_.y(),end_point.x()-robot_pose_.x());
                global_goal_angle= Funcs::NormalizeAngle(DataSet::robot_pose.angle()+local_angle+ M_PI_2); // + 90deg
            }

            if(!finished_turn)
            {
                double dst_angle= Funcs::NormalizeAngle(DataSet::robot_pose.angle() - global_goal_angle);
                if(fabs(dst_angle) >= TURN_REEOR)  // 0.052359876rad ~= 3.0deg
                {
                    TurnToAngle(linear_vel,angular_vel,dst_angle);
                    return;
                }
                else
                {
                    finished_turn=true;
                }
            }

            if(finished_turn)
            {
                finished = true;
                obs_type= NONEXIST_OBS;
                finished_turn= 
                avoid_finished=
                init_pose=
                init_angle= false;
                linear_vel=
                angular_vel=0;
            }
        }
    }

    void TopCapObsPlanner::Avoid(double &linear_vel, double &angular_vel)
    {
        if(time< 1e-5)
        {
            time=Time::Now();
            avoid_finished=false;
        }
        if( (Time::Now()-time)< 0.3)
        {
            linear_vel = avoid_speed;
            angular_vel = 0;
        }
        else
        {
            time=0;
            avoid_finished=true;
        }
    }


    void TopCapObsPlanner::TurnToAngle(double &linear_vel, double &angular_vel,double &dst_angle)
    {
            if (dst_angle > 0.17 || dst_angle < -0.17) //当角度差相差0.1弧度时    1deg=0.017453292rad
            {
                    linear_vel = 0;
                    angular_vel = -dst_angle;
            }
                else if (dst_angle > TURN_REEOR &&  dst_angle < 0.17) //当角度差小于0.1弧度时,大于0.03弧度时 
            {
                    linear_vel = 0;
                    angular_vel = -0.12;
            }
            else if (dst_angle > -0.17 && dst_angle < -TURN_REEOR) //当角度差小于0.1弧度时,大于0.052359876弧度(~3度)时
            {
                    linear_vel = 0;
                    angular_vel = 0.12;
            }
            else
            {
                    linear_vel = 0;
                    angular_vel = 0;
            }
    }



    int TopCapObsPlanner::CheakTopCapObs(PointCloud<double> &in_contour_points)
    {
        double x=0,y=0;
        int point_count = 0;
        //获得的激光雷达数据为激光雷达坐标系下的数据，需要转化到机器人坐标系
        for (auto pt : DataSet::scan_point_cloud)
        {
            //PoseF local_point_pose = PoseF(pt, 0).ConvertOriginCur(lidar_pose);
            //PointF local_point(local_point_pose.GetPoint());
            PointF local_point(PoseF(pt, 0).GetPoint());
            if (robot_shape_new.IsPointInShape(local_point))
            {
                in_contour_points.emplace_back(local_point);
                x+= local_point.GetPoint()(0);
                y+= local_point.GetPoint()(1);
            }
        }
        point_count= in_contour_points.size();
        if(point_count>POINT_NUM && !init_pose)
        {
            meanx= x/point_count;
            meany= y/point_count;
            init_pose=true;
        }
        if(point_count>1 )
        {
            avoid_speed= meanx>=0 ? -0.25:0.25;
        }
        return point_count;
    }

} // namespace behavior_controller